#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sstream>   //for converting the command line parameter to integer

using namespace std;


static string pubCameraTopic = "/camera_monitor/image";
static string cameraDevID = "";
static int pubTopicRate = 25;


int main(int argc, char** argv)
{
	string retryCameraID = "1";
	ros::init(argc, argv, "image_publisher");
	ros::NodeHandle nh;
	image_transport::ImageTransport it(nh);

	ros::param::get("~camera_deviceID", cameraDevID);
	ros::param::get("~camera_topic", pubCameraTopic);
	ros::param::get("~camera_rate", pubTopicRate);

	image_transport::Publisher pub = it.advertise(pubCameraTopic, 2);

	//Convert the passed as command line parameter index for the video device to an integer
	std::istringstream video_sourceCmd(cameraDevID.c_str());
	int video_source;

	// Check if it is indeed a number
	if(!(video_sourceCmd >> video_source))
	{
		return 1;
	}
	cv::VideoCapture cap(video_source);
	cv::VideoCapture capture(video_source);

	// Check if video device can be opened with the given index
	if(!cap.isOpened())
	{
		ROS_ERROR("Open head camera videoID:%d error will retry...", video_source);

		std::istringstream sourceCmd(retryCameraID.c_str()); 
		if(!(sourceCmd >> video_source))
		{
			ROS_ERROR("make sourceCMD >> video_source error!");
			return 1;
		}
		if(!capture.isOpened())
		{
			ROS_ERROR("Retry open head camera error!");
			return -1;
		}
	}
	ROS_INFO("Open monitor camera successfully! cameraID:%d",video_source);
	cv::Mat frame;
	sensor_msgs::ImagePtr msg;

	ros::Rate loop_rate(pubTopicRate);
	while (nh.ok())
	{
		if(video_source == 0)
		{
			cap >> frame;
		}
		else
		{
			capture >> frame;
		}
		// Check if grabbed frame is actually full with some content
		if(!frame.empty())
		{
			msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
			pub.publish(msg);
			cv::waitKey(2);
		}

		ros::spinOnce();
		loop_rate.sleep();
	}
	cap.release(); //release camera
	capture.release();
	cv::waitKey(2);

	return 0;
}

